/* USER CODE BEGIN Header */
/**
  **************************************************************************************************
  * @file           : can_motor_task.c
  * @brief          : can电机输出控制freertos任务源文件
  **************************************************************************************************
  * @attention
  *	请在 *外部变量&头文件链接区* 内链接电机用户层（can_Motor_user.c）内存储电机参数的结构体
  **************************************************************************************************
  * @needToDo
  * 在main.c中请在osKernelInitialize内核初始化函数之后调用can_motor_task_init	
  **************************************************************************************************
  */
/* USER CODE END Header */
#include "can_motor_task.h"
#include "bsp_can.h"
#include "can_motor_user.h"
#include <cmsis_os.h>
#include <cmsis_os2.h>

/*	外部变量&头文件链接区 BEGIN	*/
extern  DJI_3508   m_0x201_can1;       //底盘3508
extern  DJI_3508   m_0x202_can1;       //底盘3508
extern  DJI_3508   m_0x203_can1;       //底盘3508
extern  DJI_3508   m_0x204_can1;       //底盘3508
extern  DJI_3508   m_0x205_can1;       //防守3508

extern  DJI_3508   m_0x201_can2;       //运球3508
extern  DJI_3508   m_0x202_can2;       //运球3508
extern  DJI_3508   m_0x203_can2;       //投篮3508
extern  DJI_3508   m_0x204_can2;       //投篮3508
extern  DJI_2006   m_0x205_can2;       //投篮2006
/*	外部变量&头文件链接区 END	*/

Can_motor* can1_pointer[can1_fifo0_motor_cnt];  //方便用循环
Can_motor* can2_pointer[can2_fifo0_motor_cnt];

static inline float Speed_calc(Can_motor* m);
static inline float Angle_cal(Can_motor* m);
static inline void DJI_Motor_cmd(CAN_HandleTypeDef* hcan,uint16_t ID,int16_t c1,int16_t c2,int16_t c3,int16_t c4);

//RTOS任务参数
extern "C" void Can1ContrlTask(void *argument);
extern "C" void Can2ContrlTask(void *argument);
osThreadId_t	Can1ContrlHandle;		    //can1电机控制任务
osThreadId_t	Can2ContrlHandle;		    //can2电机控制任务
osThreadAttr_t Can1ContrlTask_attributes;
osThreadAttr_t Can2ContrlTask_attributes;
/***************************************************************************/
/*********************************函数定义区********************************/
/***************************************************************************/
//任务初始化函数
extern "C" void can_motor_task_init()
{
    can1_pointer[0] = &m_0x201_can1;        can2_pointer[0] = &m_0x201_can2;
    can1_pointer[1] = &m_0x202_can1;        can2_pointer[1] = &m_0x202_can2;
    can1_pointer[2] = &m_0x203_can1;        can2_pointer[2] = &m_0x203_can2;
    can1_pointer[3] = &m_0x204_can1;        can2_pointer[3] = &m_0x204_can2;
    can1_pointer[4] = &m_0x205_can1;        can2_pointer[4] = &m_0x205_can2;
	//创建任务
    {
        Can1ContrlTask_attributes.name = "Can1ContrlTask";
        Can1ContrlTask_attributes.stack_size = 128 * 8;
        Can1ContrlTask_attributes.priority = (osPriority_t) osPriorityBelowNormal7;    //优先级belownormal17(比normal低一级)
    }
    {
        Can2ContrlTask_attributes.name = "Can2ContrlTask";
        Can2ContrlTask_attributes.stack_size = 128 * 8;
        Can2ContrlTask_attributes.priority = (osPriority_t) osPriorityBelowNormal7;    //优先级belownormal17(比normal低一级)
    }
    Can1ContrlHandle = osThreadNew(Can1ContrlTask, NULL, &Can1ContrlTask_attributes);
    Can2ContrlHandle = osThreadNew(Can2ContrlTask, NULL, &Can2ContrlTask_attributes);
}


/**
 * @brief can1电机pid调速任务
 */
extern "C" void Can1ContrlTask(void *argument)
{
	static int16_t can1_out[can1_fifo0_motor_cnt] = {0};
    osDelay(500);   //初始化后先延时500ms
	for(;;)
	{
        for(int i = 0;i < can1_fifo0_motor_cnt;++i)
        {
            //更新数据
            can1_pointer[i]->update_measure();
            //是否让电机动
            if(can1_pointer[i] -> state  ==  Can_motor::MOTOR_STATE::RUN)
            {
                //算角度环
                if(can1_pointer[i] -> angle_mode == Can_motor::MOTOR_STATE::ANGLE_YES)
                {
                    can1_pointer[i]->target_speed = Angle_cal( can1_pointer[i] );
                }

                //算速度环
                if(i >= 0 && i <= 3)    //底盘
                {
                    can1_out[i] = can1_pointer[i]->spid.chassis_pid_calc(can1_pointer[i]->speed_rpm_now
                                            ,can1_pointer[i]->target_speed);
                }
                else
                {
                    can1_out[i] = Speed_calc( can1_pointer[i] );
                }
            }
            //不让动就输出0
            else
            {
                can1_out[i] = 0;
            }
        }
        //0x200
        DJI_Motor_cmd(&hcan1,0x200,can1_out[0],can1_out[1],can1_out[2],can1_out[3]);
        //0x1ff
        DJI_Motor_cmd(&hcan1,0x1ff,can1_out[4],0,0,0);
        //释放cpu
			osDelay(1);
	}
}


/**
 * @brief can2电机pid调速任务
 */

extern "C" void Can2ContrlTask(void *argument)
{
	static int16_t can2_out[can2_fifo0_motor_cnt] = {0};
	osDelay(500);   //初始化后先延时500ms
	for(;;)
	{
		for(int i = 0;i < can2_fifo0_motor_cnt;++i)
        {
            //更新数据
            can2_pointer[i]->update_measure();
            //是否让电机动
            if(can2_pointer[i] -> state  ==  Can_motor::MOTOR_STATE::RUN)
            {
                //算角度环
                if(can2_pointer[i] -> angle_mode == Can_motor::ANGLE_YES)
                {
                    can2_pointer[i]->target_speed = Angle_cal(can2_pointer[i]);
                }

                //算速度环
                can2_out[i] = Speed_calc( can2_pointer[i] );
            }
            //不让动就输出0
            else
	    	{
                can2_out[i] = 0;
	    	}
        }
		//0x200
		DJI_Motor_cmd(&hcan2,0x200,can2_out[0],can2_out[1],can2_out[2],can2_out[3]);
		//0x1ff
		DJI_Motor_cmd(&hcan2,0x1ff,can2_out[4],0,0,0);
		//释放cpu
		osDelay(1);
	}
}

/**
 * @brief   算速度环
 * @param   m     算哪个电机的   
 * @retval  输出的电流/电压     
 */
static inline float Speed_calc(Can_motor* m)
{
    return m -> spid.pid_calc(m -> speed_rpm_now,   m -> target_speed);
}


/**
 * @brief   算角度环
 * @param   m     算哪个电机的        
 * @retval  输出的期望速度
 */
static inline float Angle_cal(Can_motor* m)
{
    //维护总角度
    {
        // 计算原始角度差
        float delta =  m->angle_now - m->angle_last;
        // 处理360°边界情况（修正角度差方向）
        if (delta > 180.0f) {
            delta -= 360.0f;  // 正转超过180°视为反转
        } else if (delta < -180.0f) {
            delta += 360.0f;  // 反转超过180°视为正转
        }
        // 累加总角度
        m->angle_total += delta;
    }
    return m->apid.pid_calc(m-> angle_total, m-> target_angle);
}

/**
 * @brief   使用指定的can外设,对指定can ID的大疆电机发送控制报文
 * @param 	hcan 	使用的can外设					    @args   &hcan1 或者 &hcan2
 * @param	ID		电机ID							@args   0x200,0x1ff,0x2ff,0x1fe,0x2fe
 * @param 	cx		对应ID为x的电机的电流(电压)值	    @args   -10000 ~ 10000(2006电机),
													@args   -16384 ~ 16384(3508电机),
													@args   -25000 ~ 25000(6020电机电压控制),
													@args   -16384 ~ 16384(6020电机电压控制).
 * @return 	none
 */
static inline void DJI_Motor_cmd(CAN_HandleTypeDef* hcan,uint16_t ID,int16_t c1,int16_t c2,int16_t c3,int16_t c4)
{
    uint8_t can_m_cmd_tx[8] = {0};
    can_m_cmd_tx[0] = (uint8_t)(c1 >> 8);can_m_cmd_tx[1] = (uint8_t)(c1);	//ID1
    can_m_cmd_tx[2] = (uint8_t)(c2 >> 8);can_m_cmd_tx[3] = (uint8_t)(c2);	//ID2
    can_m_cmd_tx[4] = (uint8_t)(c3 >> 8);can_m_cmd_tx[5] = (uint8_t)(c3);	//ID3
    can_m_cmd_tx[6] = (uint8_t)(c4 >> 8);can_m_cmd_tx[7] = (uint8_t)(c4);	//ID4
    can_send_msg(hcan,ID,can_m_cmd_tx,8);
}




